COMPUTATIONAL GEOMETRIC OPTIMAL CONTROL OF RIGID BODIES 



TAEYOUNG LEE, MELVIN LEOK, AND N. HARRIS MCCLAMROCH 



Abstract. This paper formulates optimal control problems for rigid bodies in a geometric man- 
ner and it presents computational procedures based on this geometric formulation for numerically 
solving these optimal control problems. The dynamics of each rigid body is viewed as evolving 
on a configuration manifold that is a Lie group. Discrete-time dynamics of each rigid body are 
developed that evolve on the configuration manifold according to a discrete version of Hamilton's 
principle so that the computations preserve geometric features of the dynamics and guarantee 
evolution on the configuration manifold; these discrete-time dynamics are referred to as Lie group 
variational integrators. Rigid body optimal control problems are formulated as discrete-time op- 
timization problems for discrete Lagrangian/Hamiltonian dynamics, to which standard numerical 
optimization algorithms can be applied. This general approach is illustrated by presenting results 
for several different optimal control problems for a single rigid body and for multiple interacting 
rigid bodies. The computational advantages of the approach, that arise from correctly modeling 
the geometry, are discussed. 



1. Introduction 

This paper utilizes methods from geometric mechanics and optimal control to develop new com- 
putational procedures for geometric optimal control of rigid bodies. The emphasis is on formulat- 
ing a discrete-time optimal control problem that inherits important conservation properties of rigid 
body dynamics; this is achieved by combining variational integrators [36 and Lie group methods 16J 
to evolve the mechanical configuration. This approach leads to Lie group variational integrators 
that define the discrete-time rigid body dynamics which the optimal control computations are based 
upon [lailH]. 

Most of the prior work related to optimal control of a rigid body is based on local coordinates on 
SO (3) or quaternions [U [TTJ 32] • Minimal representations of the attitude of a rigid body, such as 
Euler angles, exhibit coordinate singularities, and require manipulating complicated trigonometric 
expressions. Nonminimal representations such as quaternions have no coordinate singularities, but 
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they also introduce certain complications. In particular, the group of unit quaternions SU(2) ~ S'^ 
double covers SO (3), so there is an ambiguity in representing an attitude of a rigid body. Further- 
more, the Hamiltonian structure of rigid body attitude dynamics is unnecessarily complicated when 
it is expressed in terms of quaternions [32j . 

By considering rigid body translation and rotation as evolution on a Lie group, optimal control 
problems defined on Lie groups were introduced by Roger Brockett ^ [3] and by John Baillieul [T] . 
They emphasized the use of Lie group structures to characterize controllability and existence of 
optimal controls; they also obtained analytical results for the solution of certain types of optimal 
control problems. An optimal control problem for a generalized rigid body on SO(n) was considered 
in [3] , and a general theory of optimal control problems on a Lie group was developed in [HI [191 EO] 
together with reachability and controllability conditions. Although these papers viewed rigid body 
translation and rotation as motion on a Lie group, their results are limited to optimal control 
problems that can be formulated solely in terms of kinematics. In particular, they do not include 
dynamics in their analysis, and assume that the controls enter directly at the level of the Lie algebra. 

The approach of computational geometric optimal control is focused on developing numerical 
algorithms, for optimal control problems, that preserve the geometric properties of the dynamics 
and the optimal control problem 22J. The essential idea is to apply geometric optimal control theory 
to discrete-time mechanical systems obtained using geometric numerical integrators. A discrete- 
time version of the generalized rigid body equations and their formulation as an optimal control 
problem are presented in [31 [S] , and discrete-time optimal control problems for the dynamics of a 
rigid body are considered in [6l[30l[25]. A direct optimal control approach is applied to discrete-time 
mechanical systems in [17 , and it is referred to as Discrete Mechanics and Optimal Control. 

This paper presents the approach of computational geometric optimal control for the dynamics 
of rigid bodies on a Lie group. We take the same geometric perspective as in the work of Roger 
Brockett |;8i |9], viewing evolution on a Lie group as fundamental. However, the emphasis in the 
present paper is on geometric formulations of both the kinematics and dynamics in the optimal 
control formulation and the role of geometric methods in optimal control computations. 

The development in the paper makes clear that there arc important advantages in formulating 
the optimal control problem as a discrete-time optimal control problem using Lie group variational 
integrators and then applying standard computational methods to solve the resulting discrete-time 
optimization problem. This is in contrast with approaches that construct continuous-time necessary 
conditions and then make use of computational methods to solve these necessary conditions. The 
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paper demonstrates that for for the optimal control of rigid bodies, the proposed approach exhibits 
important advantages. 

The main contributions of this paper can be summarized as follows; (i) the analytical and 
computational results presented in this paper are coordinate free; they avoid the singularities, 
ambiguity, and complications associated with local coordinates, and they provide a global insight 
into rigid body dynamics, (ii) a geometric optimal control problem is formulated for nontrivial rigid 
body dynamics that evolve on a Lie group, and (iii) a computational geometric optimal control 
approach is developed based on a geometric numerical integrator. 

Section [2] provides a summary of Lie group variational integrators for rigid bodies that evolve on 
a Lie group. The resulting discrete-time rigid body dynamics are used as a basis for formulating 
a discrete-time optimal control problem. In Section [3] and 21 four different examples of rigid body 
optimal control problems are studied in some detail. First, optimal orbit and attitude maneuvers 
for a rigid dumbbell spacecraft in orbit about a large central body are studied. Then, optimal 
attitude maneuvers for a 3D pendulum acting under uniform gravity are studied; the control input 
conserves the component of the vertical component of the angular momentum thereby requiring 
a careful computational treatment that avoids numerical ill-conditioning. The third example is 
a 3D pendulum attached to a cart that can move in a horizontal plane; optimal reconfiguration 
maneuvers are studied for this cart and pendulum system. The fourth example involves optimal 
attitude maneuvers of two rigid bodies connected by a universal joint; the control input conserves 
angular momentum and the resulting controlled system exhibits a symmetry that has to be taken 
into account in the numerical approach in order to avoid numerical ill-conditioning. 

2. Mathematical formulation for optimal control of rigid bodies 

The dynamics of rigid bodies exhibit important geometric features. The configuration of a rigid 
body can be described by the position vector of its center of mass in the Euclidean space M.^ and 
by the attitude of the rigid body represented by a rotation matrix in the special orthogonal group 
S0(3) ^ {Re M^^3 I R'^R = I,detR= 1}. Thus, the general motion of a rigid body is described 
by the special Euclidean group SE(3) = S0(3)(s)K'^. The configuration manifold for the class of 
multiple rigid bodies can be represented as a product involving R'^, S0(3), and SE(3). Therefore, the 
configuration manifold of rigid bodies is a Lie group. Furthermore, the dynamics of rigid bodies, 
viewed as Lagrangian or Hamiltonian systems, are characterized by symplectic, momentum and 
energy preserving properties. These geometric features determine the qualitative behavior of the 
rigid body dynamics. 
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In this paper, we study optimal control problems for rigid bodies while carefully considering the 
geometric features of the dynamics in both the analysis and numerical computations. In particular, 
discrete-time dynamics of rigid bodies are developed that evolve on the configuration manifold ac- 
cording to a discrete version of Hamilton's principle. The resulting geometric numerical integrator, 
referred to as a Lie group variational integrator, preserves geometric features of the dynamics and 
guarantees evolution on the configuration manifold. Based on the discrete-time rigid bodies dy- 
namics, a discrete-time optimal control problem for rigid bodies is formulated. Standard numerical 
optimization algorithms can then be applied to solve this discrete-time optimal control problem. 

Thus, our approach to discrete-time optimal control is characterized by discretizing the continuous- 
time optimal control problem at the problem formulation stage using Lie group variational integra- 
tors. This is in contrast to traditional techniques wherein discretization only arises at the last stage 
when numerically solving the continuous-time optimality conditions. Since the geometric properties 
of the dynamics of rigid bodies are preserved by using a Lie group variational integrator, this op- 
timal control approach yields geometrically-exact optimal control inputs and accurate trajectories 
that are efficiently computed [4l fTT l [30 l l25] . 

In this section, we first describe the fundamental procedure to develop a Lie group variational 
integrator and its computational properties. Then, a discrete-time optimal control problem is 
formulated using the Lie group variational integrator, and computational approaches are presented 
to solve it numerically. 

2.1. Lie group variational integrator. Geometric numerical integrators are numerical integra- 
tion algorithms that preserve features of the continuous-time dynamics such as invariants, symplec- 
ticity, and the configuration manifold [M] . The geometrically exact properties of the discrete-time 
flow generate improved qualitative behavior. In this paper, we view a Lie group variational inte- 
grator as an intrinsically discrete-time dynamical system. 

Numerical integration methods that preserve the simplecticity of a Hamiltonian system have 
been studied extensively 09l |32] . One traditional approach is to carefully choose the coefficients of 
a Runge-Kutta method to satisfy a simplecticity criterion and order conditions in order to obtain 
a symplectic Runge-Kutta method. However, it can be difficult to construct such integrators, and 
it is not guaranteed that other invariants of the system, such as momentum maps, are preserved. 
Alternatively, variational integrators are constructed by discretizing Hamilton's principle, rather 
than discretizing the continuous Euler-Lagrange equation |381 136j . The resulting integrators have 
the desirable property that they are symplectic and momentum preserving, and they exhibit good 
energy behavior for exponentially long times. Lie group methods are numerical integrators that 
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Figure 1 . Procedures to derive continuous-time and discrete-time equations of motion 



preserve the Lie group structure of the configuration manifold . Recently, these two approaches 
have been unified to obtain Lie group variational integrators that preserve the geometric properties 
of the dynamics as well as the Lie group structure of the configuration manifold without the use of 
local charts, reprojections, or constraints [5il [551 [?fl[^ . 

We now summarize the derivation of a Lie group variational integrator. In Lagrangian mechan- 
ics, the equations of motion are derived by finding the path that extremizes the action integral, 
which is the integral of the Lagrangian over time. The Legendre transformation provides an alter- 
native description that leads to Hamilton's equations. Discrete-time Lagrangian and Hamiltonian 
mechanics, referred to as variational integrators, have been developed by reformulating Lagrangian 
and Hamiltonian mechanics in a discrete-time setting |36j . 

Discrete-time mechanics has a parallel structure with the mechanics described in continuous-time, 
as summarized in Figure [TJ The phase variables of the continuous-time Lagrangian are replaced by 
two copies of the discrete-time configuration variables and a discrete-time Lagrangian that approx- 
imates a segment of the action integral is chosen. An action sum is defined using the discrete-time 
Lagrangian such that it approximates the action integral. This is the only approximation made in 
the development of discrete-time mechanics. Discrete-time Euler-Lagrange equations are obtained 
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by setting the variation of the action sum to zero. The discrete-time Legendre transformation yields 
the equivalent of Hamilton's equations. Lie group variational integrators are developed to preserve 
the structure of the Lie group configurations as well as the geometric properties of the continuous- 
time dynamics. The basic idea for all Lie group methods is to express the update map for group 
elements in the configuration manifold in terms of the group operation, so that the group structure 
is preserved automatically without need of parameterizations, constraints, or reprojections. 

More explicitly, consider a mechanical system whose configuration manifold is a Lie group G and 
is described by a Lagrangian L : TG M. The discrete update for the configuration is chosen as 



where gk,gk+i G G are configuration variables, and the subscript k denotes the value of a variable 
at the time t — kh for a fixed timestep /i G M. The discrete-time update map is represented by 
a right group action of e G on gk- Since the group element is updated by a group action, the 
group structure is preserved. 

The expression for the flow map in discrete-time is obtained from the discrete variational principle 
on a Lie group, as presented in Figure [TJ A discrete Lagrangian : G x G — > M approximates the 
integral of the Lagrangian over a time step along the solution of the Euler-Lagrange equation 

Ld{gk,fk)~ / L{g{t),g{t))dt, (2) 

Jkh 

where a curve g{t) : [fc/i, (fc l)h] —>■ G satisfies the Euler-Lagrange equation in the time interval 
[k, {k + with boundary conditions g{kh) — gk and g{{k + l)h) = gkfk = Qk+i- Analogous to 
the action integral, the action sum is defined as 

N-l 



The discrete Lagrange-d'Alembert principle, which is a modification of Hamilton's principle to 
include the effect of control inputs, states that the sum of the variation of the action sum and the 
virtual work done by the control inputs is zero. But, the infinitesimal variation of a Lie group 
element must be carefully expressed to respect the structure of the Lie group. For example, it can 
be expressed in terms of the exponential map exp : g — > G as 



9k+i — gkfk, 



(1) 
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for a Lie algebra element i] £ q. From the discrete Lagrange-d'Alembert principle, we obtain 

S&d + ^ [ufe • Vk+1 +u- ■7jk]^0 (5) 

fc=0 

for any Sgk, and for given discrete Lagrangian forces E g*. This yields the generalized 

discrete Euler-Poincare equation 

+ T*Lg,^i •DiLrf(.gfc+i,/fe+i) + u+ _^ + u^^ =0. (6) 

Here L/ : G ^ G denotes the left translation map given by Lfg = fg for /, 5 e G, TgL/ : TgG 
T fgG is the tangent map for the left translation and Adg : g — > g is the adjoint map. A dual map 
is denoted by a superscript * (see [35] for detailed definitions and developments). 

This approach has been applied to the rotation group S0(3) and to the special Euclidean group 
SE(3) for dynamics of rigid bodies in [27l EH EH] and the generalization to abstract Lie groups 
are summarized here, thereby generating a unified geometric integrator for the class of multiple 
generalized rigid bodies whose configuration manifold can be expressed as a Lie group, which 
includes products involving K^, S0(3), and SE(3) as special cases. 

2.2. Discrete-time optimal controL Optimal control problems involve finding a control input 
such that a certain optimality objective is achieved under prescribed constraints. Here, the control 
inputs are parameterized by their values at each discrete time step, and the discrete-time equations 
of motion, including the control inputs, are obtained from ([6]). Any standard numerical algorithm 
for constrained optimization can be applied to this discrete-time system. 

An indirect approach to solving a discrete-time optimal control problem is based on solving 
discrete-time necessary conditions for optimality. The resulting two-point boundary value prob- 
lem can be solved by using standard numerical root finding techniques; one such approach is the 
shooting method that iterates on initial values of the multipliers. Alternatively, a direct approach 
formulates the discrete-time optimal control problem as a nonlinear programming problem, which 
is solved using standard numerical optimization algorithms such as a sequential quadratic pro- 
gramming algorithm; one such approach is the DMOC (Discrete Mechanics and Optimal Control) 
approach [T7]. 

Explicit time-discretization prior to numerical optimization has significant computational advan- 
tages. As discussed in the previous section, the discrete-time dynamics are faithful representations 
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of the continuous-time dynamics, and consequently more accurate solutions to the optimal con- 
trol problems are typically obtained. The external control inputs may break the Lagrangian and 
Hamiltonian system structure; for example, the total energy may not be conserved for a controlled 
mechanical system. But, the computational superiority of the discrete mechanics formulation still 
holds for controlled systems. In particular, it has been demonstrated in [36 that the discrete-time 
dynamics derived from the discrete Lagrange-d'Alembert principle accurately computes the energy 
dissipation rate of controlled systems. For example, this feature is extremely important in accu- 
rately computing optimal trajectories for spacecraft orbit and attitude maneuvers for which the 
control authority is low and the maneuver time is large. 

The proposed discrete-time optimal control formulation provides a framework for accurate com- 
putations. In most indirect optimal control approaches, the optimal solutions are sensitive to small 
variations in the initial values of the multipliers. This may cause difhculties, such as numerical ill- 
conditioning, in solving the necessary conditions for optimality expressed as a two-point boundary 
value problem. Numerically computed sensitivity derivatives, using Lie group variational integra- 
tors, do not exhibit numerical dissipation, which typically arises in conventional numerical integra- 
tion schemes. Thus, the proposed approach leads to numerical robustness and efficient numerical 
computations. This indirect computational approach exhibits the quadratic convergence rate that 
is typical of Newton methods when it is applied to an optimal attitude control problem [29j : the 
error in satisfaction of the optimality condition converges to machine precision superlinearly. For 
the direct optimal control approach, the optimal control inputs can be parametrized using fewer 
degrees of freedom, thereby reducing the computational overhead. 

Several optimal control problems involving rigid bodies have been previously studied by the 
authors. Minimum-fuel and time-optimal control of spacecraft large-angle attitude maneuvers are 
studied in [23l [151 [30l [31] . The optimal orbit transfer of a dumbbell spacecraft, wherein the rota- 
tional attitude dynamics are non-trivially coupled to the translational dynamics, is studied in j25j . 
An underactuated optimal control problem for the attitude maneuver of a 3D pendulum is studied 
in |29| . An optimal formation reconfiguration of multiple rigid body spacecraft is studied in |26j . 
An optimal control problem for a dynamic system evolving on an abstract Lie group is developed 
in [22], thereby generating a unified approach for optimal control problems of multiple rigid bodies. 

In this paper, we summarize results for two optimal control problems for a single rigid body in 
Section [3] and results for two optimal control problems for multiple rigid bodies in Section [J] Each 
of these optimal control problems treats complex dynamics of a single or multiple rigid bodies, 
demonstrating the value of the proposed geometric optimal control approach. 
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3. Optimal control problems for a single rigid body 

3.1. Optimal maneuver of a dumbbell spacecraft on SE(3). We develop an optimal 3D 
translational and rotational maneuver of a rigid dumbbell spacecraft in orbit about a large central 
body. The dumbbell spacecraft is composed of two spheres connected by a massless rod. An 
interesting feature of the dumbbell spacecraft is that there is coupling between its translational 
dynamics and its rotational dynamics due to the presence of both gravity forces and gravity moments 
that act on the dumbbell spacecraft. 

The configuration manifold is the special Euclidean group SE(3) = S0(3)@M'^. For {R,x) G 
SE(3), the linear transformation from the body-fixed frame to the inertial frame is denoted by the 
rotation matrix R £ SO (3), and the position of the mass center in the inertial frame is denoted 
by a vector x G R'^. The vectors £7,1; G R'^ are the angular velocity in the body-fixed frame, and 
the translational velocity in the inertial frame, respectively. Let m G R and J G R"^^^ be the mass 
and the moment of inertia matrix of a rigid body. We assume that external control force u-^ G R'^ 
and control moment G R'^ act on the dumbbell spacecraft. Control inputs are parameterized by 
their values at each time step. 

Define a ft = (F^, Yfc) G SE(3) such that gk+i = {Rk+i,Xk+i) is equal to gkfk, i-e. {Rk+i,Xk+i) = 
{Rk,Xk) o (FkjYk) — {RkFk,Xk + RkYk)- The rotation matrix Fk represent the relative update of 



the attitude between integration steps. The gravitational potential is denoted by U : SE(3) R. 
We choose the following discrete Lagrangian 

Ld{Rk,xu,Fk,Yk) = ^mYifYk + itr[(/ - Fk)Jd] - hU{RkFk,Xk + RuYk), (7) 

where Jd G R'^^'^ is a non-standard moment of inertia matrix defined as Jd — ^tr[J]/3x3 — J. 
Substituting this discrete Lagrangian into ([6]) , we obtain the following discrete equations of motion 
(see [22] for detailed development). 

hJQk ^FkJd- JdF^, (8) 

Rk+l = RkFk, (9) 

Xk+i = Xk + hvk, (10) 

JQk+i - Fi'jnk + h{Mk+i + <+i), (11) 

mvk+i ^ mvk - h^^^ + hul^^, (12) 
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where the hat map " is an isomorphism from M'^ to 3 x 3 skew-symmetric matrices so (3), defined 
such that xy = X X y for any x,y ^R^. The moment M G M"^ due to the potential is given by, 

where the matrix G R'^^'^ is defined by = '^^^ ^ {1:2,3}, and the i, j-th element 

of a matrix is denoted by 

For a given {Rk,Xk,^k,Vk), we solve the implicit equation ([5]) to find Fk £ S0(3). Then, the 
configuration at the next step {Rk+i, Xk+i) is obtained from ([9]) and (fTO| . Using the computed 
moment Mk+i and force —^^r^, velocities V,k+i,Vk+i are obtained from pT|) and p^ . This 
defines a discrete flow map, {Rk,Xk,^k,Vk) ^ {Rk+i,Xk+i,^k+i-,Vk+i), and this process can be 
repeated. 

Since this Lie group variational integrator is obtained by discretizing Hamilton's principle, it is 
symplectic and preserves the momentum map associated with the symmetry of the Lagrangian. In 
the absence of external forces and moments, the total energy oscillates around its initial value with 
small bounds on a comparatively short timescale, but there is no tendency for the mean of the 
oscillation in the total energy to drift (increase or decrease) from the initial value for exponentially 
long times. 

The discrete flow map also preserves the group structure. By using the given computational 
approach, the matrix Fk^ representing the change in relative attitude change over a time step, is 
guaranteed to be a rotation matrix. The rotation matrix Rk+i is obtained by the group operation 
in lU, so that it evolves on SO (3). Therefore, the orthogonal structure of the rotation matrices is 
preserved, and the attitude of each rigid body is determined accurately and globally. 

This geometrically exact numerical integration method yields a highly efficient computational 
algorithm. The self-adjoint discrete Lagrangian used to derive this Lie group variational integrator 
guarantees that this integrator has second-order accuracy, while requiring only one function eval- 
uation per integration step. Higher-order methods can be easily constructed using a composition 
method [14]. 

An implicit equation ([8|) must be solved at each time step to determine the attitude update. 
However the computational effort to solve each implicit equation is negligible; the relative attitude 
update is expressed at the Lie algebra level isomorphic to R^, and the corresponding Newton 
iteration converges to machine precision within two or three iterations. This method could be 
considered almost explicit when the computational cost is compared with explicit integrators with 
the same order of accuracy [37] ■ 
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Optimal control problem. The objective is to transfer the spacecraft from a given initial condition 
{Ro, xo,^0:^o) to a desired terminal condition {R-^ , x-^ , Q-^ , v-^ ) during a fixed maneuver time Nh, 
while minimizing the square of the I2 norm of the control inputs. 

f h h ) 

min ^7 = ^ ^{ui+,fWfui^, + -{uT+ifW^uT+, , (14) 

where Wf,Wm G M'^^^ are symmetric positive-definite matrices. 



Necessary conditions for optimality. An indirect optimization method is used to determine the 
optimal solution, based on necessary conditions for optimality derived using variational arguments; 
the optimal control is characterized as a solution of a two-point boundary value problem. The 
augmented cost function to be minimized is 

Af-l - 



fc=0 



, 1 '■^^k+.^n.u, ■^Q^^^^^--k+i 

+ XY (logm(Ffe - RlRk+i)Y + {-.mu+i + + h {A'h+i + <+i)} , (15) 

where A^,,A^,A|,A^ G M'^ are Lagrange multipliers. The matrix logarithm is denoted by logm : 
S0(3) 50(3) and the vee map V : so(3) R'^ is the inverse of the hat map. The logarithmic 
form of ^ is used, and the constraint ([H]) is implicitly imposed using constrained variations. Using 
similar expressions for the variations given in ([4]), the infinitesimal variation of the cost can be 
written as 

AT-l 

5Ja = ^^''i'^ {^/"fc + ^'-1} + '^^"^'^ {WmU^ + XU} + zl {'Xk-l + AlXk) , (16) 

fe=l 

where Afc = [Afei A^; A|; A|] G M}'^ is the vector of Lagrange multipliers, and Zk G M^^ represents 
the infinitesimal variation of {Rk,Xk,^k,Vk), given by Zk = [logm{Rj SRk)"^ ; 6xk, Sflk, Svk]- The 
matrix Ak G M^^'*^^ is expressed in terms of {Rk,Xk,^k,Vk) Thus, necessary conditions for 

optimality are given by 

4+1 = 'Wf'>^l (17) 
<Vi = -W,V,'Xt, (18) 
Xk = A^+iAfc+i (19) 
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together with the discrete equations of motion and the boundary conditions. 

Computational approach. Necessary conditions for optimahty are expressed in terms of a two-point 
boundary problem. This problem is to find the optimal discrete flow, multipliers, and control inputs 
that simultaneously satisfies the equations of motion, optimality conditions, multiplier equations, 
and boundary conditions. We use a neighboring extremal method [lOj, and choose a nominal so- 
lution satisfying all of the necessary conditions except the boundary conditions. The unspecified 
initial multiplier is updated by successive linearization so as to satisfy the specified terminal bound- 
ary conditions in the limit. This is also referred to as a shooting method. The main advantage of 
the neighboring extremal method is that the number of iteration variables is small. 

The difficulty is that the extremal solutions are sensitive to small changes in the unspecified 
initial multiplier values. The nonlinearities also make it hard to construct an accurate estimate of 
sensitivity, thereby resulting in numerical ill-conditioning. Therefore, it is important to compute 
the sensitivities accurately in the neighboring extremal method. Here, the optimality conditions 
(jl7|) and (jlSp are substituted into the equations of motion and the multiplier equations, which are 
linearized to obtain 
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where € R^^^ for i.j € {1, 2} represents a computable linear operator. For the given two-point 
boundary value problem, zq — since the initial condition is fixed. The terminal multipliers are 
free. Thus, we obtain 

The linear operator ^E"^^ represents the sensitivity of the specified terminal boundary conditions 
with respect to the unspecified initial multiplier. Using this sensitivity, a guess of the unspecified 
initial multipliers is iterated to satisfy the specified terminal conditions in the limit. Any type of 
Newton iteration can be applied. We use a line search with backtracking algorithm, referred to as 
the Newton- Armijo iteration [21]. 

Numerical example. We study a maneuver of a rigid spacecraft under a central gravity field. We 
assume that the mass of the spacecraft is negligible compared to the mass of a central body, and 
we consider a fixed frame attached to the central body as an inertial frame. The resulting model is 
a Restricted Full Two Body Problem (RF2BP) [40]. 
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The spacecraft is modeled as a dumbbell, which consists of two equally massive spheres and a 
massless rod. The gravitational potential is given by 

2 



U{R,x) 



GMr 



9=1 



1 



\x + Rpi\ 



(20) 



where G S R is the gravitational constant, M,m £ M are the mass of the central body, and the 
mass of the dumbbell, respectively. The vector p'^ G M.^ is the position of the qth sphere from the 
mass center of the dumbbell expressed in the body fixed frame (q G {1,2}). The mass, length, 
and time dimensions are normalized by the mass of the dumbbell, the radius of a reference circular 
orbit, and its orbital period. 

Initially, the spacecraft is on a circular orbit. The desired maneuver is to increase the orbital 
inclination by 60°. We explicitly consider the coupling effect between the orbital motion and the 
rotational attitude maneuver of the spacecraft. The maneuver time is chosen to be a quarter of the 
orbital period of the initial circular orbit. The boundary conditions are as follows. 
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Figure [3 illustrates the optimal spacecraft maneuver, convergence rate, and optimal control 
inputs. The optimal cost and the violation of the terminal boundary conditions are 13.03, and 9.32 x 
10^^^ respectively. Figure 2(b) shows the violation of the terminal boundary conditions versus the 
number of iterations on a semi-logarithmic scale. Red circles denote outer iterations of the Newton- 
Armijo iteration where the sensitivity derivatives are computed, and inner iterations correspond 
to backtracking in the line search routine. The initial guess of the unspecified initial multipliers is 
arbitrarily chosen. The error in satisfaction of the terminal boundary condition converges quickly 
to machine precision after the 20th iteration. These convergence results are consistent with the 
quadratic convergence rates expected of Newton methods with accurately computed gradients. 

The shooting method may be prone to numerical ill-conditioning, as a small change in the initial 
multiplier can cause highly nonlinear behavior of the terminal conditions. However, as shown in 



Figure 2(b) the computational geometric optimal control approach exhibits excellent numerical 
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Figure 2. Optimal orbit transfer of a dumbbell spacecraft 



convergence properties. This is because the proposed computational algorithms are geometrically 
exact and numerically accurate. There is no numerical dissipation introduced by the numerical 
algorithm, and therefore, the sensitivity derivatives are more accurately computed. 

3.2. Optimal attitude reorientation of an underactuated 3D pendulum on S0(3) [29 . 
A 3D pendulum is a rigid body supported by a fixed frictionless pivot acting under the influence 
of a uniform gravitational field [43 . The rigid body has three rotational degrees of freedom, and 
the configuration manifold is SO (3). The linear transformation from the body fixed frame and the 
inertial frame is denoted by i? S SO (3), and the angular velocity represented in the body fixed frame 
is denoted by e K."^. Let 63 G R"^ be the gravity direction in the inertial frame, and J G M'^^'^ be 
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the moment of inertia matrix of the rigid body with respect to the pivot point. The vector from 
the pivot point to the mass center, represented in the body fixed frame is given by p G M'^. 

The Lagrangian of the 3D pendulum is invariant under a rotation about the gravity direction, 
and therefore the 3D pendulum has a symmetry action. Consequently, the angular momentum 
about the gravity direction, represented by ejRJil, is preserved. 

We study an optimal attitude control of the 3D pendulum with symmetry. An external control 
moment is chosen such that it does not have any component about the gravity direction. The 
structure of the control moment is chosen as R"^ x u for a control parameter m G R'^. Thus, the 
angular momentum about the gravity direction is conserved along the controlled dynamics of the 
3D pendulum. Such control inputs are physically realized by actuation mechanisms, such as point 
mass actuators, that change the center of mass of the 3D pendulum. 

The discrete Lagrangian of the 3D pendulum is chosen to be 

LdiRk,Fk) = itr[(/ - Fk)Jd] + hmgelRp. 
The resulting Lie group variational integrator, including an external control input, is given by 

hMk = FkJd ~ JdF^, (21) 

Rk+l = RkFk, (22) 

JQk+i = F^Jflk + hMk+i + hR^ 63 X Uk+i- (23) 

Optimal control problem. The objective of the optimal control problem is to transfer the 3D pendu- 
lum from a given initial condition {Rq, fio) to a desired terminal condition {R-^ , fl^) during a fixed 
maneuver time Nh, while minimizing the square of the I2 norm of the control inputs. 

mm\j=S2-{uk+ifWuk+i}, (24) 

(. k=0 } 

where S R'^^'^ is a symmetric positive-definite matrix. In particular, we choose attitude maneu- 
vers that can be described by rest-to-rest rotations about the unactuated gravity direction. The 
resulting optimal attitude maneuver exhibits the geometric phase effect [35] , which in the zero group 
momentum case directly relates the group motion to the curvature enclosed by the trajectory in 
shape space. 

Necessary conditions for optimality. We solve this optimal control problem by using an indirect 
optimization method, where necessary conditions for optimality are derived using variational argu- 
ments, and a solution of the corresponding two-point boundary value problem provides the optimal 
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control. The augmented cost function to be minimized is 

+ {-J^k+i + F^.mk + hMk+i + hRl^^ez x Uk+i] , (25) 
where A^, \\ G 9? are Lagrange multipHers. The infinitesimal variation can be written as 

N-l 

5Ja = J2 ^^"^l {^"fc - ^fc ^3 ^ ^fe-l} + {-^k-l + A^Afc} , (26) 

k=l 

where = ['^Is^'^fe] ^ i ^fc £ 11^^ represents the infinitesimal variation of (i?fc,ilfc), given 
by Zfc = [logm(i?^5i?fc)^; (5ilfe]. The matrix G M^^® can be expressed in terms of {Rk,^k), ^k- 
Thus, necessary conditions for optimality are given by 

Uk+i^W-\Rl+,e3xXl), (27) 
= ^fc+iAfe+1 (28) 

together with the discrete equations of motion and the boundary conditions. 

Computational approach. We apply the neighboring extremal method described in Section I3.lt the 
optimality condition is substituted into the equations of motion and the multiplier equation, and 
sensitivity derivatives of the optimal solution with respect to the initial multiplier are obtained, 
and the initial multiplier is iterated to satisfy the terminal boundary condition. 

However, the underactuated control input, that respects the symmetry of the 3D pendulum, 
causes a fundamental singularity in the sensitivity derivatives, since the controlled system inherits 
the symmetry, and the cost functional is invariant under the lifted action of S^. Consequently, 
the sensitivity derivatives vanish in the group direction. At each iteration, we need to compute 
inverse of a matrix of sensitivity derivatives to update the initial multiplier. However, the sensi- 
tivity matrix has a theoretical rank deficiency of one since the vertical component of the inertial 
angular momentum is conserved regardless of the initial multiplier variation. Therefore, this matrix 
inversion is numerically ill-conditioned. 

We present a simple numerical scheme to avoid the numerical ill-conditioning caused by this 
symmetry. At each step, we decompose the matrix of sensitivity derivatives into a symmetric part 
and an anti-symmetric part. The symmetric part describes the sensitivity of the conserved angular 
momentum component due to the symmetry, and therefore it is zero and does not depend on the 
initial multiplier values. An update for the initial multipliers is determined using the matrix inverse 
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of the anti-symmetric part; this matrix inverse is not iU-conditioned. This approach removes the 
singularity in the sensitivity derivatives completely, and the resulting optimal control problem is no 
longer ill-conditioned. 

Numerical example. Properties of the 3D pendulum are chosen as, 

m = lkg, J = diag[0.13,0.28,0.17]kgm^ and p = [0, 0, 0.3] m. 

The desired maneuver is a 180° rotation about the vertical axis from a hanging equilibrium to 
another hanging equilibrium. The corresponding boundary conditions are given by 

i?o = /, i?^ =diag[-l,-l,l], 

fio = [0,0,0], = [0,0,0]. 

The maneuver time is 1 second, and the time step is h — 0.001. Since the vertical component of 
the angular momentum is zero, the rotation is a consequence of the geometric phase effect [35^. 
This problem is challenging in the sense that the desired maneuvers are rotations about the gravity 
direction, but the control input docs not directly generate any moment about the gravity direction. 

Figure [3] illustrates the optimal pendulum maneuver, convergence rate, and optimal control 
inputs. The optimal cost and the violation of the terminal boundary conditions are 7.32, and 



4.80 X 10 respectively. As shown in Figure 3(c) the error in satisfaction of the terminal bound 



ary condition converges to machine precision after the 50th iteration. The condition number of 
the decomposed sensitivity derivative varies from lO'^ to 10^. If the sensitivity derivative is not 
decomposed, then the condition numbers are at the level of 10^®, and the numerical iterations fail 
to converge. This numerical example demonstrates the excellent numerical convergence proper- 
ties of the computational geometric optimal control approach that is achieved by incorporating a 
modification that eliminates the numerical ill-conditioning introduced by the symmetry. 

4. Optimal control problems for multiple rigid bodies 

4.1. Optimal maneuver of a 3D pendulum on a 2D cart on S0(3) x K'^. Consider a 3D 
pendulum whose pivot is attached to a cart that can translate on a horizontal plane. This is 
a generalization of the popular planar pendulum on a cart model (see, for example, [7]), where 
the pendulum has three rotational degrees of freedom, and the cart moves on a two dimensional 
horizontal plane. 

We define two frames; an inertial frame and a body fixed frame for the 3D pendulum whose 
origin is located at the moving pivot point. Define 
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a; G K Displacement of the cart along the ei direction in the reference frame 

2/ G K Displacement of the cart along the 62 direction in the reference frame 

R G S0(3) Rotation matrix from the body fixed frame to the reference frame 

$7 G M'^ Angular velocity of the pendulum represented in the body fixed frame 

d G Vector from the pivot to the mass center of the pendulum represented in the body 

fixed frame 
m G M Mass of the pendulum 
M G K Mass of the cart 

The configuration manifold is S0(3) x M^. We assume that external control forces u^^Uy G M are 

applied to the cart. 

The Lagrangian of the 3D pendulum on a cart is invariant under a rotation about the gravity 
direction. Therefore, it has a symmetry of action, and the total angular momentum about the 
gravity direction is preserved. The external control forces acting on the cart break this symmetry, 
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and the controlled system is not symmetric. In particular, the total angular momentum is not 
preserved in the controlled dynamics. Therefore, this optimal control problem should be distin- 
guished from the optimal control of a 3D pendulum with symmetry, discussed in Section [321 where 
a symmetry-preserving control input is chosen. 

The discrete Lagrangian for the 3D pendulum on a 2D cart is 

Ld{Rk,Xk,yk,Rk+i,Xk+i,yk+i) = ^i^'^ + m){{xk+i - XkY + {Vk+i - VkY) 

1 TTt TTl 

+ ^tr[(/ - Fk)Jd\ + —{xk+i - Xk)eJ{Rk+i - Rk)d+ -j^iVk+i - yk)eJiRk+i - Rk)d 



^mge'^Rkd+ !^mge^ Rk+id. 



(29) 



From ([S]), the Lie group variational integrator for the 3D pendulum on a cart is given by the 
discrete-time equations 



1 m 

-(M -I- m)(Xk+i - Xk) + —ei(Rk+i - Rk)d, 

1 TTl 

-{M + m){yk+i - Uk) + -re2{Rk+i - Rk)d, 



TTl . \ T TT\ . \ T h rp 

— (xfc+i - Xk)dRk ei -I- — [yk+i - VkjdR,, 62 - -mgdR,, 63 



Rk+l 


= RkFki 




— Pxk + hUx^^i, 


Pvk + l 


= Pvk + ^'^Vk + n 


P^k+i 


= J^iJdFk - F^Jd) 



(30) 
(31) 

(32) 
(33) 
(34) 
(35) 

A 



lib. \ 1 T' lit, \^T^ 1 T' 

— (xfc+i - Xk)dRk+^ei + -j^KVk+i - yk)dRk+ie2 + -mgdRf,+ie:i 



(36) 



The momenta variables pn G , , G M are given by 



Pn 




Px 




Py. 





J mdR^ei radRF e2 
-me[Rd M + m 
-me^Rd M + m 





'n 




X 







(37) 



The detailed derivation of this Lie group variational integrator is available in [22j . For given 
{Rk,Xk:yk,^k,Xk,yk), we compute {pn^,Pxk,Pyk) by ([37]). We use a fixed-point iteration to 
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compute Rk+1- For an initial guess for Rk+i, the corresponding Xk+i,yk+i are obtained by us- 
ing pO|) .(|3T |) . Then, we can find Fk by solving p2|) . The updated value for Rk+i is given by 
([551) . This is repeated until Rk+i converges. Then, Xk+i,yk+i are obtained from ([50)1 . ([51]) . and 
{pnk+i^Pxk+nPyk+i) are obtained by ([M]) , ([31]) , and The velocities {nk+i,Xk+i,yk+i) are ob- 

tained from ([37]). This yields a flow map. 



Optimal control problem. The objective of the optimal control problem is to transfer the 3D pen- 
dulum on a cart from a given initial condition (Rq, XQ,yQ,Clg,XQ, {jq) to a desired terminal condition 
{R^ ,x-^ ,y-^ ,n-^ ,x-^ ,y-^) during a fixed maneuver time Nh, while minimizing the square of the I2 
norm of the control inputs. 



where Uk = [u^i^iUy^] G K^, and W £ R^^^ is a symmetric positive-definite matrix. The 3D 
pendulum on a cart is underactuated, since only the planar motion of the cart in its horizontal 
plane is actuated. 

Computational approach. We apply a direct optimal control approach. The control inputs are 
parameterized by several points that are uniformly distributed over the maneuver time, and control 
inputs between these points are approximated using a cubic spline interpolation. For given control 
input parameters, the value of the cost is given by (|38p. and the terminal conditions are obtained 
by the discrete-time equations of motion given by (|30p - ([5^ . The control input parameters are 
optimized using constrained nonlinear parameter optimization to satisfy the terminal boundary 
conditions while minimizing the cost. 

This approach is computationally efficient when compared to the usual collocation methods, 
where the continuous-time equations of motion are imposed as constraints at a set of collocation 
points. Using the proposed discrete-time optimal control approach, optimal control inputs can be 
obtained by using a large step size, thereby resulting in efficient total computations. Since the 
computed optimal trajectories do not have numerical dissipation caused by conventional numerical 
integration schemes, they are numerically more robust. Furthermore, the corresponding gradient 
information is accurately computed, which improves the convergence properties of the numerical 
optimization procedure. 



{Rk,Xk,yk,^k,ik,yk) {Rk+i,Xk+i,yk+i,^k+i,Xk+i,yk+i)- 




(38) 
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Numerical example. Properties of the 3D pendulum and the cart are chosen as, 

M = m = 1kg, J = diag[1.03,1.04,0.03]kgm2, and d=[0,0, l]m. 

The desired maneuver is a rest-to-rest 180° rotation of the pendulum about the vertical axis, while 
the cart returns to the initial location at the terminal time. The corresponding boundary conditions 
are given by 

i?o = I, f^o = [0, 0, 0], xo = yo = 0, io = yo = 0, 
Rf = diag[-l, -1, 1], = [0, 0, 0], xf = i/ = 0, x^ = y^ = 0. 

The maneuver time is 2 seconds, and the time step is h = 0.01. Since only the planar motion of 
the cart is actuated, the rotation of the 3D pendulum is caused by the nonlinear coupling between 
the cart and the pendulum. 





(a) Optimal maneuver of a 3D pendulum on a cart 
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(c) Angular velocity H 



Figure 4. Optimal control of a 3D pendulum on a cart 
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Each component of the control inputs is parameterized by 7 points. The resulting 14 control 
input parameters are optimized using sequential quadratic programming. Figure |4] illustrates the 
optimal maneuver of the pendulum and the cart, angular velocity, and optimal control inputs. The 
blue circles denote the optimized control input parameters. The optimal cost and the violation of 
the terminal boundary conditions are 297.43, and 1.83 x 10^*, respectively. The optimal motion of 
the cart on the horizontal plane consists of a triangular-shaped loop, and the optimal maneuver of 
the 3D pendulum consists of large angle rotations. This also demonstrates the advantages of the 
computational geometric optimal control approach: it is difficult to study this kind of aggressive 
maneuvers of a multibody system using local coordinates, due to the coordinate singularities and the 
complexity of the equations in local coordinates. The presented computational geometric optimal 
control approach accurately characterizes the nonlinear coupling between the cart and the pendulum 
dynamics to obtain a nontrivial optimal maneuver of the 3D pendulum on a cart. 

4.2. Optimal attitude reorientation of two connected rigid bodies on S0(3) x S0(3). 
Consider two rigid bodies connected with a ball joint that has three rotational degrees of freedom. 
This represents a freely rotating system of coupled rigid bodies. The relative equilibria structure 
of this rigid body dynamics has been studied in [44]. We introduce three frames; an inertial frame 
and two body-fixed frames. Define 



a; G Position of the ball joint in a reference frame 

Ri £ SO (3) Rotation matrix from the i-th body- fixed frame to a reference frame 
di e Vector from the joint to the mass center of the i-th body in the i-th body-fixed frame 

TOi e M Mass of the i-th body 
for i S {1,2}. The configuration manifold is S0(3) x S0(3) x R'^. In the absence of the potential 
field, the connected rigid body model has two symmetries; a symmetry of the translational action 
of R'^, and a symmetry of the rotational action of SO (3)0 Due to these symmetries, the total linear 
momentum and the total angular momentum are preserved, and the configuration manifold can be 
reduced to a quotient space. 

In this optimal control problem, we reduce the configuration manifold to S0(3) x S0(3) using the 
symmetry of the translational action of . The corresponding value of the total linear momentum 
is set to zero. The resulting connected rigid bodies model with a fixed mass center is closely related 
to the falling cat problem T3]. An appropriate cyclic change in the shape of the body yields a 



^These can be considered as a single symmetry of the translational and rotational action of SE(3), but they are 
considered separately in this optimal control problem. By the general theory of reduction by stages | 12| , the two 
approaches are equivalent. 
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rotation in the orientation of the cat in accordance with the geometric phase effect [37] . In contrast 
to other models of the faUing cat, which typicaUy introduce two one-dimensional rotational joints, 
with a shape space given by x S^, we consider instead a single ball joint with a shape space 
given by 5*0(3). 

Similar to the falling cat problem, we assume that an internal control moment u G M"^ is applied 
at the joint, so that it controls the relative attitude between two rigid bodies. More precisely, 
the control input u represents the control moment applied to the first rigid body, represented in 
the reference frame. The equal and opposite control moment is applied to the second rigid body. 
Therefore the control moment changes the shape of the system. The total angular momentum is 
conserved for the controlled dynamics as the control input is an internal moment of the connected 
rigid bodies system. This optimal control problem is similar to the optimal control problem of 
the 3D pendulum discussed in Section 13.21 as the control input respects the symmetry, and the 
corresponding momentum is preserved in the controlled dynamics. 

The discrete Lagrangian for the two connected rigid bodies is 

Ld{Ri^,Fi^,R2^,F2^,Xk,Xk+i) ^ "^1 +"^-2 (^^^^ _ . (^^^^ _ 2;^) + ■)-tr[(/3x3 - -FiJJdJ 



From ([S]), we obtain the Lie group variational integrator, viewed as discrete-time equations of 
motion on S0(3) x S0(3) x M'^. Since we are only interested in rotational maneuvers, we derive the 
following reduced equations of motion on SO (3) x SO (3) using the fact that the linear momentum 
is conserved. 




P2, 




{Jdi - amidid[)F[^ } 



(Jd, ~ Pm2d2d^)Fl} 



did^FlRlR,,) + (3!I^{RlR2j2dJ 



d2d^FlRlR2,) + 



(40) 



d2rff<i?2j, (41) 



Ri 



k+1 



(42) 



i?2 



'k+1 



(43) 



PU+i 



(44) 
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where a — 



nil 



mi+m2 



mi +m2 



and the matrix Bi, G 



3x3 



for i G {1, 2} is defined as 



B^, - -^{F,, - I)d,{-aRi,{F,, - I)d, - PR2dF2, - R^. 



The momenta variables pi , P2 G are given by 



(45) 



(46) 



(47) 



For given {Ri^,, R2^,ili^,ft2i-), we find pi^,p2^ by (HT)) . We solve the implicit equations (HOI), 
(|4T|) to obtain Fi^,F2^. Then, , i?2fc+i are obtained from ([iS]) . (|43|) . and pij^^j,p2fc+i are ob- 

tained by (|44| ■ (|45|) . Finally, fii^.^^ , r22fc+i are computed from ([47]) . This yields a discrete flow map 

(i?lj, , i?2fc , f^lfc , f^2 J ^ (^U + i,-R2fc + i,f^U + i,f^2fc+i)- 



Pi 




,/i + aniidf 


/3midiRj R2d2 






P2 




ani2d2 R2 Ridi 


J2 + I3m2dl 




VI2 



Optimal control problem. The objective of the optimal control problem is to transfer the con- 
nected rigid bodies from a given initial condition (i?!,, , i?2o i ^lo : ^2o ) to a desired terminal condition 
(i?{, iij , ^^2) during a fixed maneuver time A^/i, while minimizing the square of the I2 norm of 
the control inputs. 

C jv-i 



min {J = ^ ^ul^^Wuk+i 



(48) 



A;=0 



where W G M.^^^ is a symmetric positive-definite matrix. In particular, we choose an attitude 
maneuver that is described by a rest-to-rest rotation of the entire system while the relative attitude 
configuration at the terminal time is the same as at the initial time. 



Computational approach. We apply a direct optimal control approach. For a given control input, 
the value of the cost is given by , and the terminal conditions are obtained by the discrete-time 
equations of motion given by (I40p - (l46p . We use constrained nonlinear parameter optimization to 
minimize the cost function subject to the terminal boundary condition obtained by the discrete-time 
equations of motion. 

Since the total angular momentum is conserved regardless of the control input, the terminal 
constraints introduces a singularity due to the rotational symmetry. This ill-conditioning can be 
avoided by disregarding the terminal angular velocity constraint for the second body. For the given 
boundary conditions, the terminal angular velocity condition is automatically satisfied if the re- 
maining terminal constraints are satisfied, due to the angular momentum conservation property. By 
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formulating the optimization process this way, we ehminate the source of numerical ill-conditioning. 
This is similar to the modified computational approach discussed in Section [3?2l 

Numerical example. Properties of the rigid bodies are chosen as 



mi = l-5kg, Ji 



0.18 0.32 0.32 
0.32 1.88 -0.06 
0.32 -0.06 1.86 



kg • m^, di = [-1.08, 0.20, 0.20]m, 



TO2 = 1kg, J2 = 



0.11 -0.18 -0.18 
-0.18 0.89 -0.04 
-0.18 -0.04 0.88 



kg-m^ d2 = [0.9,0.2,0.2]m. 



The desired maneuver is a rest-to-rest 180° rotation about the x axis. 



-Rio — I, ^la — 0, i?2a 



i?( =diag[l,-l,-l], 17{ = [0,0,0], i?^ =diag[l,-l,-l], r!^ = [0,0,0]. 

The maneuver time is 4 seconds, and the step size is h = 0.01. 

We parameterize each component of the control input at 7 discrete points, and the control 
inputs are reconstructed by cubic spline interpolation. The resulting 21 control input parameters 
are optimized by a sequential quadratic programming method to satisfy the terminal boundary 
conditions while minimizing the cost function. 

Figure [5] shows the optimal maneuver of the rigid bodies, angular velocity, and optimal control 
inputs. The blue circles denote the optimized control input parameters. The optimal cost and the 
violation of the terminal boundary conditions are 0.574, and 2.48 x 10~®, respectively. 

The optimal maneuver consists of large angle rotations of the two rigid bodies. Throughout this 
complicated maneuver, the total angular momentum is zero, and the rotation about the ei axis 
depends on the geometric phase effect. This also demonstrates the advantages of the computational 
geometric optimal control approach. The Lie group variational integrator computes the weak geo- 
metric phase effect accurately, so that the iterations converge to a nontrivial optimal maneuver of 
the coupled rigid bodies. 

5. Conclusions 

In this paper, a computational geometric approach for an optimal control problem of rigid body 
dynamics has been developed. The essential idea is formulating a discrete-time optimal control 
problem using a structure-preserving geometric numerical integrator, referred to as a Lie group 
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(b) Control input u (c) Angular velocity (f2i;solid, n2:dashed) 



Figure 5. Optimal control of two connected rigid bodies 

variational integrator, and applying standard optimal control approaches, such as an indirect opti- 
mal control and a direct optimal control, to discrete-time equations of motion. This method is in 
contrast to the usual optimal control approach, where the discretization appears only in the last 
stage when numerically computing the optimal control inputs. 

The computational geometric optimal control approach has substantial advantages in terms 
of preserving the geometric properties of optimality conditions. The discrete flow of Lie group 
variational integrators has desirable geometric properties, such as symplecticity and momentum 
preservation, and it is more reliable and robust over longer time periods. The computational 
geometric optimal control approach inherits the desirable properties of the Lie group variational 
integrator. In the necessary conditions for optimality, the multiplier equations are dual to the 
linearized equations of motion. Since the linearized flow of a Lagrangian/Hamiltonian system is 
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also symplectic, the multiplier equations inherit certain geometric properties. The discrete-time 
necessary conditions preserve the geometric properties of the optimality conditions, as they are 
derived from a discrete-time analogue of Hamilton's variational principle that yields a symplectic 
discrete-time flow. 

The computational geometric optimal control approach allows us to find the optimal control input 
more efficiently. In the indirect optimal control, the shooting method may be prone to numerical 
ill-conditioning, as a small change in the initial multiplier can cause highly nonlinear behavior of the 



terminal condition. However, as shown in Figure 2(b) and Figure 3(c) the computational geometric 



optimal control approach exhibits excellent numerical convergence properties. This is because the 
proposed computational algorithms are geometrically exact and numerically accurate. There is no 
numerical dissipation introduced by the numerical algorithm, and therefore, we are more accurately 
characterizing the sensitivities along the solution. 

Another advantage of the computational geometric optimal control of rigid bodies is that the 
method is directly developed on a Lie group. There is no ambiguity or singularity in representing 
the configuration of rigid bodies globally, and the resulting equations of motion are more compact 



than those written in terms of local coordinates. As illustrated by Figure 4(a) and Figure 5(a) 
the presented computational geometric optimal control approach utilizes the effects of the nonlin- 
ear coupling and the weak geometric phase of a multibody system to obtain nontrivial aggressive 
maneuvers of the rigid bodies. These results are independent of a specific choice of local coordi- 
nates, and they completely avoid any singularity, ambiguity, and complexity associated with local 
coordinates. Furthermore, the numerical results are group-equi variant, and are independent of the 
choice of inertial frame, which is in contrast to methods based on local coordinate representations. 
By formulating the problem in a global and intrinsic fashion, the algorithms presented are able 
to explore the space of control strategies which extend beyond a single coordinate chart, thereby 
providing a deeper insight into the global controlled dynamics of systems of rigid bodies. 
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